
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_sensors.c
  * @author     baiyang
  * @date       2022-10-7
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <sensor_rangefinder/sensor_rangefinder.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// return true if rangefinder_alt can be used
bool fms_rangefinder_alt_ok()
{
    return (fms.rangefinder_state.enabled && fms.rangefinder_state.alt_healthy);
}

// return true if rangefinder_alt can be used
bool fms_rangefinder_up_ok()
{
    return (fms.rangefinder_up_state.enabled && fms.rangefinder_up_state.alt_healthy);
}

/*
  get inertially interpolated rangefinder height. Inertial height is
  recorded whenever we update the rangefinder height, then we use the
  difference between the inertial height at that time and the current
  inertial height to give us interpolation of height from rangefinder
 */
bool fms_get_rangefinder_height_interpolated_cm(int32_t* ret)
{
    if (!fms_rangefinder_alt_ok()) {
        return false;
    }
    *ret = lpf_get_output(&fms.rangefinder_state.alt_cm_filt);
    float inertial_alt_cm = ahrs_get_position_z_up_cm(fms.ahrs);
    *ret += inertial_alt_cm - fms.rangefinder_state.inertial_alt_cm;
    return true;
}

void fms_init_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
   sensor_rngfnd_set_log_rfnd_bit(MASK_LOG_CTUN);
   sensor_rngfnd_init();
   lpf_set_cutoff1(&fms.rangefinder_state.alt_cm_filt, fms.g.rangefinder_filt);
   fms.rangefinder_state.enabled = sensor_rngfnd_has_orientation(ROTATION_PITCH_270);

   // upward facing range finder
   lpf_set_cutoff1(&fms.rangefinder_up_state.alt_cm_filt, fms.g.rangefinder_filt);
   fms.rangefinder_up_state.enabled = sensor_rngfnd_has_orientation(ROTATION_PITCH_90);
#endif
}

// return rangefinder altitude in centimeters
void fms_read_rangefinder(void)
{
#if RANGEFINDER_ENABLED == ENABLED
    sensor_rngfnd_update();

#if RANGEFINDER_TILT_CORRECTION == ENABLED
    const float tilt_correction = MAX(0.707f, fms.ahrs->cos_roll * fms.ahrs->cos_pitch);
#else
    const float tilt_correction = 1.0f;
#endif

    // iterate through downward and upward facing lidar
    struct {
        RangeFinderState *state;
        enum RotationEnum orientation;
    } rngfnd[2] = {{&fms.rangefinder_state, ROTATION_PITCH_270}, {&fms.rangefinder_up_state, ROTATION_PITCH_90}};

    for (uint8_t i=0; i < ARRAY_SIZE(rngfnd); i++) {
        // local variables to make accessing simpler
        RangeFinderState *rf_state = rngfnd[i].state;
        enum RotationEnum rf_orient = rngfnd[i].orientation;

        // update health
        rf_state->alt_healthy = ((sensor_rngfnd_status_orient(rf_orient) == RangeFinderGood) &&
                                (sensor_rngfnd_range_valid_count_orient(rf_orient) >= RANGEFINDER_HEALTH_MAX));

        // tilt corrected but unfiltered, not glitch protected alt
        rf_state->alt_cm = tilt_correction * sensor_rngfnd_distance_cm_orient(rf_orient);

        // remember inertial alt to allow us to interpolate rangefinder
        rf_state->inertial_alt_cm = ahrs_get_position_z_up_cm(fms.ahrs);

        // glitch handling.  rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
        // are considered a glitch and glitch_count becomes non-zero
        // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row.
        // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset
        const int32_t glitch_cm = rf_state->alt_cm - rf_state->alt_cm_glitch_protected;
        bool reset_terrain_offset = false;
        if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) {
            rf_state->glitch_count = MAX(rf_state->glitch_count+1, 1);
        } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) {
            rf_state->glitch_count = MIN(rf_state->glitch_count-1, -1);
        } else {
            rf_state->glitch_count = 0;
            rf_state->alt_cm_glitch_protected = rf_state->alt_cm;
        }
        if (abs(rf_state->glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) {
            // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes
            rf_state->glitch_count = 0;
            rf_state->alt_cm_glitch_protected = rf_state->alt_cm;
            rf_state->glitch_cleared_ms = time_millis();
            reset_terrain_offset = true;
        }

        // filter rangefinder altitude
        uint32_t now = time_millis();
        const bool timed_out = now - rf_state->last_healthy_ms > RANGEFINDER_TIMEOUT_MS;
        if (rf_state->alt_healthy) {
            if (timed_out) {
                // reset filter if we haven't used it within the last second
                lpf_reset(&rf_state->alt_cm_filt, rf_state->alt_cm);
                reset_terrain_offset = true;
            } else {
                lpf_apply2(&rf_state->alt_cm_filt, rf_state->alt_cm, 0.05f);
            }
            rf_state->last_healthy_ms = now;
        }

        // handle reset of terrain offset
        if (reset_terrain_offset) {
            if (rf_orient == ROTATION_PITCH_90) {
                // upward facing
                rf_state->terrain_offset_cm = rf_state->inertial_alt_cm + rf_state->alt_cm;
            } else {
                // assume downward facing
                rf_state->terrain_offset_cm = rf_state->inertial_alt_cm - rf_state->alt_cm;
            }
        }

        // send downward facing lidar altitude and health to the libraries that require it
#if 0
        if (rf_orient == ROTATION_PITCH_270) {
            if (fms.rangefinder_state.alt_healthy || timed_out) {
                g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
            }
        }
#endif
    }

#else
    // downward facing rangefinder
    fms.rangefinder_state.enabled = false;
    fms.rangefinder_state.alt_healthy = false;
    fms.rangefinder_state.alt_cm = 0;

    // upward facing rangefinder
    fms.rangefinder_up_state.enabled = false;
    fms.rangefinder_up_state.alt_healthy = false;
    fms.rangefinder_up_state.alt_cm = 0;
#endif
}

void fms_update_rangefinder_terrain_offset()
{
    float terrain_offset_cm = fms.rangefinder_state.inertial_alt_cm - fms.rangefinder_state.alt_cm_glitch_protected;
    fms.rangefinder_state.terrain_offset_cm += (terrain_offset_cm - fms.rangefinder_state.terrain_offset_cm) * (fms.G_Dt / MAX(fms.g.surftrak_tc, fms.G_Dt));

    terrain_offset_cm = fms.rangefinder_up_state.inertial_alt_cm + fms.rangefinder_up_state.alt_cm_glitch_protected;
    fms.rangefinder_up_state.terrain_offset_cm += (terrain_offset_cm - fms.rangefinder_up_state.terrain_offset_cm) * (fms.G_Dt / MAX(fms.g.surftrak_tc, fms.G_Dt));

    if (fms.rangefinder_state.alt_healthy || (time_millis() - fms.rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS)) {
        wpnav_set_rangefinder_terrain_offset(fms.wp_nav, fms.rangefinder_state.enabled, fms.rangefinder_state.alt_healthy, fms.rangefinder_state.terrain_offset_cm);
#if 0 // MODE_CIRCLE_ENABLED
        fms.circle_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
#endif
    }
}

/*------------------------------------test------------------------------------*/


